#include "routine.h"
#include <math.h>
#include "encoder.h"
#include "motor.h"

/* 私有函数声明 */
static void Calculate_Motors_Speed(float radius, uint8_t direction, uint16_t *left_speed, uint16_t *right_speed);
static void Wait_For_Distance(float target_distance_cm);

/* 直线运动函数 */
void Move_Straight(float distance)
{

    // 设置两个电机以相同速度向前运动
    Motor_SetSpeed(MOTORA, base_speed, MOTOR_FORWARD);  // 左电机
    Motor_SetSpeed(MOTORB, base_speed, MOTOR_FORWARD);  // 右电机

    // 等待直到达到目标距离
    Wait_For_Distance(distance);

    // 停止电机
    Motor_Stop(MOTORA);
    Motor_Stop(MOTORB);

    // 短暂延时确保完全停止
    HAL_Delay(100);
}

/* 圆弧运动函数 */
void Move_Arc(float radius, float angle, uint8_t direction)
{
    uint16_t left_speed, right_speed;

    // 确保角度在1-360度范围内
    if(angle < 1.0f) angle = 1.0f;
    if(angle > 360.0f) angle = 360.0f;

    // 计算左右轮速度
    Calculate_Motors_Speed(radius, direction, &left_speed, &right_speed);

    // 计算弧长
    float outer_radius = radius + (wheel_spacing / 2.0f);
    float arc_length = (outer_radius * angle * 3.14159f) / 180.0f;  // 弧长(cm)

    // 设置电机速度
    if(direction == TURN_LEFT)
    {
        Motor_SetSpeed(MOTORA, left_speed, MOTOR_FORWARD);   // 左轮
        Motor_SetSpeed(MOTORB, right_speed, MOTOR_FORWARD);  // 右轮
    }
    else if(direction == TURN_RIGHT)
    {
        Motor_SetSpeed(MOTORA, right_speed, MOTOR_FORWARD);  // 左轮
        Motor_SetSpeed(MOTORB, left_speed, MOTOR_FORWARD);   // 右轮
    }
    else {
        Motor_Stop(MOTORA);
        Motor_Stop(MOTORB);
    }

    // 等待直到达到目标距离（使用外圈轮子的行走距离）
    Wait_For_Distance(arc_length);

    // 停止电机
    Motor_Stop(MOTORA);
    Motor_Stop(MOTORB);

    // 短暂延时确保完全停止
    HAL_Delay(100);
}

/* 开始执行路径 */
void Start_Routine(void)
{
    // 示例路径：
    // 1. 直线前进50cm
    Move_Straight(50.0f);

    // 2. 以30cm半径顺时针转90度
    Move_Arc(30.0f, 90.0f, TURN_LEFT);

    // 3. 再次直线前进30cm
    Move_Straight(30.0f);
}

/* 计算圆弧运动时左右轮的速度 */
static void Calculate_Motors_Speed(float radius, uint8_t direction, uint16_t *left_speed, uint16_t *right_speed)
{
    float inner_radius = radius - (wheel_spacing / 2.0f);
    float outer_radius = radius + (wheel_spacing / 2.0f);
    float ratio = inner_radius / outer_radius;

    // 使用基础速度计算
    if(direction == TURN_LEFT)
    {
        *right_speed = base_speed;              // 外轮
        *left_speed = (uint16_t)(base_speed * ratio);  // 内轮
    }
    else if(direction == TURN_RIGHT)
    {
        *left_speed = base_speed;               // 外轮
        *right_speed = (uint16_t)(base_speed * ratio); // 内轮
    }
    else {
        *left_speed = base_speed;               // 外轮
        *right_speed = base_speed;              // 内轮
    }
}

/* 等待直到达到目标距离 */
static void Wait_For_Distance(float target_distance_cm)
{
    do {

    } while(current_distance < target_distance_cm);
}
